/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "WirelessMap.h"

CWirelessMap::CWirelessMap(CWirelessPositionLog wLog, 
													 std::string linemapname, float resolution)
{
	///TODO: Add set/get													 
	RSS_range = 1.5; ///< The range that a single measurement affects
	RSS_weight = 0.5; ///< The weighting for RSS measurement
	
	
	grid = new TOccupancyGrid(10, 10, resolution, Grid::TCoord2D(0,0));
	grid->loadLineMap(linemapname.c_str(), 1.0,255,true,true,0);
	grid->getDim(Nx,Ny);
		
	measurementCost = (float *) malloc(Nx*Ny*sizeof(float));
	resetCost();		
	///Go through all keys
	for(unsigned int i=0; i<wLog.signalKeys.size();i++){
			std::pair<std::multimap<uint64_t, SignalStrengthWithPosition>::iterator, 
			std::multimap<uint64_t, SignalStrengthWithPosition>::iterator> partial;
			
			TWirelessCellData *wdata = new TWirelessCellData[Nx*Ny]; ///Signal map for this id
			for(int j=0;j<Nx*Ny;j++){
				wdata[j].w = 0;
				wdata[j].rss=0;
			}
			
			
			partial = wLog.signalMap.equal_range(wLog.signalKeys[i]); ///Get all values of this key

			for (std::multimap<uint64_t, SignalStrengthWithPosition>::iterator it2 = partial.first;
							it2 != partial.second; ++it2)
			{
				dPrint(10,"%llx=%s %f %f %d",
							 (*it2).first,(*it2).second.ESSID.c_str(), 
							 (*it2).second.x,(*it2).second.y, (*it2).second.rss);
				
				setWirelessMeasurement(wdata,(*it2).second.x,(*it2).second.y,(*it2).second.rss);
			}
		
			wirelessMapData[wLog.signalKeys[i]]=wdata;
	}

}

CWirelessMap::~CWirelessMap(){
	
}

void CWirelessMap::setWirelessMeasurement(TWirelessCellData *wdata,
																					float x, 
																					float y, 
																					uint32_t rss)
{
	Grid::TPixel p=grid->toPixel(Grid::TCoord2D(x,y));
	//setFilledCircle(g,p.x,p.y,RSS_range,grid_resolution,rss);
	float grid_resolution = grid->getResolution();
	
	setBlock(wdata,p.x,p.y,RSS_range,grid_resolution,rss);
}


void CWirelessMap::setBlock(TWirelessCellData *wdata, 
														int pix, 
														int piy, 
														float r, 
														float resolution,
														float RSS)
{
	int n=(int)(r/resolution);
	for(int y=piy-n;y<piy+n;y++){
		for(int x = pix-n;x<pix+n;x++){
			float r2 = ((x-pix)*(x-pix)+(y-piy)*(y-piy))*resolution*resolution;
			float w = exp(-r2/(0.6*0.6));
			wdata[Nx*y+x].updateValue(RSS, w);
			//updatePixelValue(g,x,y,RSS,w);
		}
	}
}

void CWirelessMap::toRGB(uint64_t key, unsigned char *rgb){
	TWirelessCellData *wdata = wirelessMapData[key];
	float *dptr = grid->getDataPtr();
	
	for(int i=0;i<Nx*Ny;i++){
		if(dptr[i]!=255) dptr[i] = wdata[i].rss;
	}
	
	grid->fillRGB(rgb,1.0);
	
	for(int i=0;i<Nx*Ny;i++){
		if(rgb[3*i]>2*85){
			rgb[3*i+1] = 3*(rgb[3*i]-2*85);
			rgb[3*i] = 0;
			rgb[3*i+2] = 0;
		}else if(rgb[3*i]<85){
			rgb[3*i] = 3*rgb[3*i];
			rgb[3*i+1] = 0;
			rgb[3*i+2] = 0;
		}else{
			rgb[3*i+2] = 3*(rgb[3*i]-85);
			rgb[3*i] = 0;
			rgb[3*i+1] = 0;
		}
	}
	
}

/**
 * Calculates cost matrix using one wireless measurement
 **/
void CWirelessMap::computeWirelessMeasurementCost(MaCI::Wireless::CWirelessData *wd){
	MaCI::Wireless::TNodeIDPtrArray idArray = wd->GetNodeIDArray();
	MaCI::Wireless::TNodeSignalPtrArray signalArray = wd->GetNodeSignalArray();
	resetCost();
	for(unsigned int j=0;j<idArray.size();j++){
		TWirelessCellData *wdata = wirelessMapData[idArray[j]->nodeid];
		
		for(int i=0;i<Nx*Ny;i++){
			measurementCost[i]+=(signalArray[j]->signal_level-wdata[i].rss)*
																(signalArray[j]->signal_level-wdata[i].rss);
		}
	}
}

void CWirelessMap::costToRGB(unsigned char *rgb){
	float maxi=measurementCost[0];
	float mini=measurementCost[0];
	int minind=0;
	for(int i=0;i<Nx*Ny;i++){
		checkCostVector(measurementCost[i], i, 40);
		if(measurementCost[i]<mini){
			mini=measurementCost[i];
			minind = i;
		}
		if(measurementCost[i]>maxi) maxi = measurementCost[i];	
	}
	//fprintf(stderr,"Min=%f, max=%f\n",mini,maxi);
	for(int i=0;i<Nx*Ny;i++){
		
		float v = (measurementCost[i]-mini); //(maxi-mini) * 255.0;
		//if(v>10*(mini)) v = 10*mini;
		//v = v/(10*mini) * 255;
		if(v>255) v=255;
		rgb[3*i]=v;
		rgb[3*i+1]=v;
		rgb[3*i+2]=v;
	}
	
	Grid::TCoord2D apos;
	for(int i=0;i<minCostVector.size();i++){
		Grid::TCoord2D p=grid->toCoord(Grid::TPixel(minCostIndexVector[i]%Nx,
																	 									minCostIndexVector[i]/Nx));
		apos.x+=p.x;
		apos.y+=p.y;
		//fprintf(stderr,"%d ",minCostIndexVector[i]);
		rgb[3*minCostIndexVector[i]]=255;
		rgb[3*minCostIndexVector[i]+1]=255;
		rgb[3*minCostIndexVector[i]+2]=0;
	}
	apos.x=apos.x/minCostVector.size();
	apos.y=apos.y/minCostVector.size();
	Grid::TPixel apix=grid->toPixel(apos);
	//fprintf(stderr,"\n");
	rgb[3*(apix.x+apix.y*Nx)]=0;
	rgb[3*(apix.x+apix.y*Nx)+1]=255;
	rgb[3*(apix.x+apix.y*Nx)+2]=255;
	
	rgb[3*minind]=255;
	rgb[3*minind+1]=0;
	rgb[3*minind+2]=0;
}


void CWirelessMap::resetCost(){
	for(int i=0;i<Nx*Ny;i++) measurementCost[i]=0;
	minCostVector.clear();
	minCostIndexVector.clear();
}

void CWirelessMap::checkCostVector(float cost, int index, int maxnum){
	if(minCostVector.size()<maxnum){
		minCostVector.push_back(cost);
		minCostIndexVector.push_back(index);
	}else{
		int maxi=0;
		float maxval=-1;
		///Max of vector
		for(int i=0;i<minCostVector.size();i++){
			if(minCostVector[i]>maxval){
				maxval = minCostVector[i];
				maxi = i;
			}
		}
		if(minCostVector[maxi]>cost){
			minCostVector.erase(minCostVector.begin()+maxi);
			minCostIndexVector.erase(minCostIndexVector.begin()+maxi);
			minCostVector.push_back(cost);
			minCostIndexVector.push_back(index);
		}
	}
}